#include <chrono>
#include <cmath>
#include <functional>
#include <memory>
#include <string>
#include <vector>

#include <iostream>
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/bool.hpp"
#include "std_msgs/msg/int16.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "mavros_msgs/msg/rc_in.hpp"
#include "customized_msgs/msg/can_motor_cmd.hpp"
#include "customized_msgs/msg/multi_can_motor_cmd.hpp"
#include "customized_msgs/msg/arg_set_length.hpp"
#include "customized_msgs/msg/arg_set_angle.hpp"
#include "std_msgs/msg/empty.hpp"
#include "customized_msgs/msg/ground_detection.hpp"
#include "customized_msgs/msg/feed_back.hpp"
#include "customized_msgs/msg/winch_state.hpp"
#include "std_msgs/msg/float32.hpp"
#include "customized_msgs/msg/double_filtered.hpp"
#include "sensor_msgs/msg/range.hpp"
// #include "customized_msgs/msg/drone_state_gcs.hpp"
#include <customized_msgs/msg/drone_whole_state.hpp>

// #define REF_TORQUE 2.0
// #define BACK_TORQUE -0.3
// #define SWIM_TORQUE 2.5

// #define PRECISE_ZERO_MODE 0 //0 for torque mode, 1 for tof mode
#define PRECISE_ZERO_MODE 1 // 0 for torque mode, 1 for tof mode

class WinchController : public rclcpp::Node
{
public:
    
    WinchController() : Node("winch_controller")
    {
        this->declare_parameter<double>("controller_freq", 20.0);
        this->declare_parameter<double>("MANUAL_SPEED", 0.09);
        this->declare_parameter<double>("accleration", 0.045);
        this->declare_parameter<double>("hold_speed", 0.135);
        this->declare_parameter<double>("max_velocity", 0.9);
        this->declare_parameter<double>("min_velocity", 0.09);
        // this->declare_parameter<double>("velocity", 0.09);
        this->declare_parameter<double>("set_length_speed", 0.09);
        this->declare_parameter<double>("critical_d_torque", -1.0);
        this->declare_parameter<double>("precise_zero_step_min", 0.002);
        this->declare_parameter<double>("precise_zero_step_max", 0.01);
        this->declare_parameter<double>("swimming_torque", 0.9);
        this->declare_parameter<double>("BACK_TORQUE", -0.3);
        this->declare_parameter<double>("fly_height", 20);          // 大飞机离地高度
        this->declare_parameter<double>("tag_optimal_distance", 4); // 下降到距离tag最佳抓取距离
        this->declare_parameter<double>("reservedHeight", 3);       // 预留高度

        // Get parameters
        this->get_parameter("controller_freq", controller_freq);
        this->get_parameter("MANUAL_SPEED", MANUAL_SPEED);
        this->get_parameter("accleration", accleration);
        this->get_parameter("hold_speed", hold_speed);
        this->get_parameter("max_velocity", max_velocity);
        this->get_parameter("min_velocity", min_velocity);
        // this->get_parameter("velocity", velocity);
        this->get_parameter("set_length_speed", set_length_speed);
        this->get_parameter("critical_d_torque", critical_d_torque);
        this->get_parameter("precise_zero_step_min", precise_zero_step_min);
        this->get_parameter("precise_zero_step_max", precise_zero_step_max);
        this->get_parameter("swimming_torque", swimming_torque);
        this->get_parameter("BACK_TORQUE", BACK_TORQUE);
        this->get_parameter("fly_height", fly_height);
        this->get_parameter("tag_optimal_distance", tag_optimal_distance);
        this->get_parameter("reservedHeight", reservedHeight);

        velocity = min_velocity;
        precise_zero_step_min = precise_zero_step_min * 100.0 / controller_freq;
        precise_zero_step_max = precise_zero_step_max * 100.0 / controller_freq;
        RCLCPP_INFO(this->get_logger(), "critial_d_torque = %f, precise_zero_step_min = %f", critical_d_torque, precise_zero_step_min);
        delta_time = 1 / controller_freq;
        set_angle_msg.angle = 0;
        set_length_msg.length = 0;

        motor_cmd.multi_can_motor_cmd[0].id = 3;
        motor_cmd.multi_can_motor_cmd[0].control_mode = 0;
        motor_cmd.multi_can_motor_cmd[0].ref_torque = 0;
        motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
        motor_cmd.multi_can_motor_cmd[0].target_angle = 0;

        motor_cmd.multi_can_motor_cmd[1].id = 4;
        motor_cmd.multi_can_motor_cmd[1].control_mode = 1;
        motor_cmd.multi_can_motor_cmd[1].ref_torque = 0;
        motor_cmd.multi_can_motor_cmd[1].target_velocity = 0;
        motor_cmd.multi_can_motor_cmd[1].target_angle = 0;

        motor_cmd.multi_can_motor_cmd[2].id = 0;
        motor_cmd.multi_can_motor_cmd[2].control_mode = 0;
        motor_cmd.multi_can_motor_cmd[2].ref_torque = 0;
        motor_cmd.multi_can_motor_cmd[2].target_velocity = 0;
        motor_cmd.multi_can_motor_cmd[2].target_angle = 0;

        motor_cmd.multi_can_motor_cmd[3].id = 0;
        motor_cmd.multi_can_motor_cmd[3].control_mode = 0;
        motor_cmd.multi_can_motor_cmd[3].ref_torque = 0;
        motor_cmd.multi_can_motor_cmd[3].target_velocity = 0;
        motor_cmd.multi_can_motor_cmd[3].target_angle = 0;

        MANUAL_SPEED /= radius;
        hold_speed /= radius;
        accleration /= radius;
        velocity /= radius;
        max_velocity /= radius;
        min_velocity /= radius;
        set_length_speed /= radius;

        RCLCPP_INFO(this->get_logger(), "controller_freq = %f", controller_freq);
        RCLCPP_INFO(this->get_logger(), "MANUAL_SPEED = %f", MANUAL_SPEED);
        RCLCPP_INFO(this->get_logger(), "accleration = %f", accleration);
        RCLCPP_INFO(this->get_logger(), "velocity = %f", velocity);
        RCLCPP_INFO(this->get_logger(), "max_velocity = %f", max_velocity);
        RCLCPP_INFO(this->get_logger(), "min_velocity = %f", min_velocity);

        // Initialize subscribers
        drone_odom_sub = this->create_subscription<nav_msgs::msg::Odometry>(
            "/winch_wireless_server/grabber_odom", 10, std::bind(&WinchController::droneOdomCallback, this, std::placeholders::_1));
        set_zero_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/winch_wireless_server/set_zero", 10, std::bind(&WinchController::setZeroCallback, this, std::placeholders::_1));
        motor_state_sub = this->create_subscription<customized_msgs::msg::FeedBack>(
            "/motor_feedback/motor1_winch", 10, std::bind(&WinchController::motorStateCallback, this, std::placeholders::_1));
        virtual_rc_sub = this->create_subscription<std_msgs::msg::Float32>(
            "/gcs_client_winch/virtual_rc", 10, std::bind(&WinchController::virtualRcCallback, this, std::placeholders::_1));
        precise_zero_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/winch_ctrl/precise_zero", 10, std::bind(&WinchController::preciseZeroCallback, this, std::placeholders::_1));
        set_angle_sub = this->create_subscription<customized_msgs::msg::ArgSetAngle>(
            "/set_angle", 10, std::bind(&WinchController::setAngleCallback, this, std::placeholders::_1));
        rc_in_sub = this->create_subscription<mavros_msgs::msg::RCIn>(
            "/mavros/rc/in", 10, std::bind(&WinchController::rcInCallback, this, std::placeholders::_1));
        set_length_sub = this->create_subscription<customized_msgs::msg::ArgSetLength>(
            "/winch_wireless_server/set_length", 10, std::bind(&WinchController::setLengthCallback, this, std::placeholders::_1));
        // ground_height_sub = this->create_subscription<customized_msgs::msg::GroundDetection>(
        //     "/ground_detector/ground_height_winch", 10, std::bind(&WinchController::groundHeightCallback, this, std::placeholders::_1));
        rappel_near_ground_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/winch_ctrl/rappel_near_ground", 10, std::bind(&WinchController::rappelNearGroundCallback, this, std::placeholders::_1));
        rappel_touch_ground_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/winch_ctrl/rappel_touch_ground", 10, std::bind(&WinchController::rappelTouchGroundCallback, this, std::placeholders::_1));
        catch_up_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/winch_ctrl/catch_up", 10, std::bind(&WinchController::catchUpCallback, this, std::placeholders::_1));
        tof_dis_sub = this->create_subscription<sensor_msgs::msg::Range>(
            "/tof_detector/tof_distance", 10, std::bind(&WinchController::tofDisCallback, this, std::placeholders::_1));
        motor_reset_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/winch_wireless_server/motor_reset", 10, std::bind(&WinchController::motorResetCallback, this, std::placeholders::_1));
        emergency_stop_sub = this->create_subscription<std_msgs::msg::Empty>(
            "/tof_detector/emergency_stop", 10, std::bind(&WinchController::emergencyStopCallback, this, std::placeholders::_1));
        limit_switch_sub = this->create_subscription<std_msgs::msg::Bool>(
            "/gpio_driver/switch_trigger", 10, std::bind(&WinchController::limitSwitchCallback, this, std::placeholders::_1));

        drone_state_sub = this->create_subscription<customized_msgs::msg::DroneWholeState>(
            "/winch_wireless_server/drone_whole_state", 1, std::bind(&WinchController::droneStateCallback, this, std::placeholders::_1));

        // Initialize publishers
        winch_ctrl_pub = this->create_publisher<customized_msgs::msg::MultiCanMotorCmd>("/winch_wireless_server/multi_can_motor_cmd", 10);
        winch_state_pub = this->create_publisher<customized_msgs::msg::WinchState>("/winch_ctrl_winch/winch_state", 10);
        set_zero_pub = this->create_publisher<std_msgs::msg::Empty>("/winch_wireless_server/set_zero", 10);

        // Initialize timers
        timer1 = this->create_wall_timer(
            std::chrono::milliseconds(10), std::bind(&WinchController::controlLoopCallback, this));
        // update_vel_timer = this->create_wall_timer(
        //     std::chrono::milliseconds(100), std::bind(&WinchController::updateVelCallback, this));

        RCLCPP_INFO(this->get_logger(), "winch_ctrl node started");
        // Initialize other member variables
        init_time = this->now();
    }
    // 限位开关相关
    void limitSwitchCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {

        if (msg->data == true)
        {
            RCLCPP_INFO(this->get_logger(), "limit switch triggered");
            go_up_lock = true; // 禁止回收
            std_msgs::msg::Empty empty_msg;
            set_zero_pub->publish(empty_msg);
            RCLCPP_INFO(this->get_logger(), "publish set_zero msg");
        }
        else
        {
            RCLCPP_INFO(this->get_logger(), "limit switch released");
            go_up_lock = false;
        }
    }

    void emergencyStopCallback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        change_control_state("EMERGENCY_STOP");
        RCLCPP_WARN(this->get_logger(), "Emergency stop triggered!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
    }
    // 绞盘重置 但是这里重置只是停止
    void motorResetCallback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        target_angle = 0;
        motor_cmd.multi_can_motor_cmd[0].target_angle = 0;
        motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
        change_control_state("RC", true);
        RCLCPP_WARN(this->get_logger(), "Motor reset");
    }

    void tofDisCallback(const sensor_msgs::msg::Range::SharedPtr msg)
    {
        tof_ready_flag = true;
        tof_dis_msg = *msg;
    }
    // 准确回零 通过tof
    void precise_zero_process_by_tof()
    {
        if (tof_ready_flag)
        {
            rclcpp::Time now = this->now();
            rclcpp::Time tof_dis_time(tof_dis_msg.header.stamp);
            double elapsed_time = (now - tof_dis_time).seconds();
            if (elapsed_time > 0.5)
            {
                RCLCPP_WARN(this->get_logger(), "tof timeout! skip precise_zero_process_by_tof");
                tof_ready_flag = false;
                return;
            }

            double dis = tof_dis_msg.range;
            double critical_dis = 0.2;
            double critical_dis_2 = 0.4;

            if (dis > critical_dis && dis < critical_dis_2)
            {
                RCLCPP_INFO(this->get_logger(), "dis = %f", dis);
                target_angle -= precise_zero_step_min;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 5;
            }
            else if (dis > critical_dis_2)
            {
                RCLCPP_INFO(this->get_logger(), "dis = %f", dis);
                target_angle -= precise_zero_step_max;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 5;
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "precise zero finished!!");
                change_control_state("RC");
                precise_zero_finished_flag = true;
            }
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "tof not ready! skip precise_zero_process_by_tof");
        }
    }
    // 准确回零 通过力矩
    void precise_zero_process_by_torque()
    {
        if (motor_state_ready_flag)
        {
            rclcpp::Time now = this->now();
            rclcpp::Time motor_state_time(motor_state_msg.stamp);
            double elapsed_time = (now - motor_state_time).seconds();
            if (elapsed_time > 0.5)
            {
                RCLCPP_WARN(this->get_logger(), "Motor feedback timeout! Skip precise_zero_process_by_torque");
                motor_state_ready_flag = false;
                return;
            }

            double d_torque = motor_state_msg.feedback_torque_filtered - precise_zero_init_torque;
            if (d_torque > critical_d_torque)
            {
                target_angle -= precise_zero_step_min;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 5;
            }
            else
            {
                target_angle += 0.2;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = set_length_speed;
                RCLCPP_WARN(this->get_logger(), "Precise zero finished!!");
                precise_zero_finished_flag = true;
                change_control_state("RC");
            }
            // RCLCPP_INFO(this->get_logger(), "d_torque = %f", d_torque);
            return;
        }
        else
        {
            RCLCPP_WARN(this->get_logger(), "Motor state feedback is not ready! Skip precise_zero_process_by_torque");
        }
    }
    // 角度转换至长度
    double angle_to_meter(double angle)
    {
        // double k1 = 0.0595;
        // double k2 = 2.1272e-05; // 8009 with 3mm nyron rope
        double k1 = 0.0433;
        double k2 = 1.1933e-05; // 8009 with 3mm nyron rope
        double output = k1 * angle - k2 * angle * angle;

        return output;
    }
    // 长度转换至角度
    double meter_to_angle(double meter)
    {
        double k1 = 0.0433;
        double k2 = 1.1933e-05; // 8009 with 3mm nylon rope

        double delta = k1 * k1 - 4 * k2 * meter;
        if (delta > 0)
        {
            double output = (k1 - sqrt(delta)) / (2 * k2);
            // RCLCPP_INFO(this->get_logger(), "input_meter = %f, output_angle = %f", meter, output);
            return output;
        }
        else
        {
            RCLCPP_ERROR(this->get_logger(), "delta is negative, meter = %f", meter);
            return (k1) / (2 * k2);
        }
    }
    // 增加长度转换至角度
    double angle_plus_meter_to_angle(double angle, double plus_meter)
    {
        double current_meter = angle_to_meter(angle);
        double new_meter = current_meter + plus_meter;
        return meter_to_angle(new_meter);
    }
    // 改变控制状态
    void change_control_state(int target_state)
    {
        if (control_state != target_state)
        {
            RCLCPP_INFO(this->get_logger(), "\033[1;32m change control state from %s to %s \033[0m",
                        control_state_str[control_state].c_str(),
                        control_state_str[target_state].c_str());
        }
        control_state = target_state;
        swimming_torque_state = true;
    }

    void change_control_state(std::string target_state_str, bool override_flag = false)
    {
        if (control_state_str[control_state] == "EMERGENCY_STOP" && !override_flag)
        {
            RCLCPP_WARN(this->get_logger(), "IN EMERGENCY_STOP STATE; REFUSE TO CHANGE STATE TO %s", target_state_str.c_str());
            return;
        }

        for (size_t i = 0; i < control_state_str.size(); ++i)
        {
            if (control_state_str[i] == target_state_str)
            {
                change_control_state(i);
                return;
            }
        }

        RCLCPP_WARN(this->get_logger(), "Target state %s not found in control_state_str", target_state_str.c_str());
    }

    void preciseZeroCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data)
        {
            if (PRECISE_ZERO_MODE == 0)
            {
                if (motor_state_ready_flag)
                {
                    if (!last_precise_zero_flag)
                    {
                        precise_zero_init_torque = motor_state_msg.feedback_torque_filtered;
                        RCLCPP_WARN(this->get_logger(), "torque init = %f", precise_zero_init_torque);
                        precise_zero_finished_flag = false;
                    }
                    if (!precise_zero_finished_flag)
                        change_control_state("PRECISE_ZERO");
                }
                else
                {
                    RCLCPP_WARN(this->get_logger(), "motor state feedback is not ready!");
                }
            }
            else if (PRECISE_ZERO_MODE == 1)
            {
                if (tof_ready_flag)
                {
                    if (!last_precise_zero_flag)
                    {
                        precise_zero_finished_flag = false;
                    }
                    if (!precise_zero_finished_flag)
                        change_control_state("PRECISE_ZERO");
                }
                else
                {
                    RCLCPP_WARN(this->get_logger(), "tof is not ready!");
                }
            }
        }
        else
        {
            if (last_precise_zero_flag)
                change_control_state("RC");
        }

        last_precise_zero_flag = msg->data;
    }
    // 接受近地下放信号，近地下放
    void rappelNearGroundCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data == true)
        {
            rappel_near_ground();
            // autoRopeRelease=true;
        }
    }
    // 接受放到地上信号，放到地上
    void rappelTouchGroundCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data)
        {
            if (motor_state_ready_flag)
            {
                change_control_state("TOUCH_GROUND");
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "motor state feedback is not ready!");
            }
        }
    }
    // 抓起信号，抓起
    void catchUpCallback(const std_msgs::msg::Bool::SharedPtr msg)
    {
        if (msg->data == true)
        {
            if (motor_state_ready_flag)
                change_control_state("CATCH_UP");

            else
            {
                RCLCPP_WARN(this->get_logger(), "motor state feedback is not ready!");
            }
        }
    }
    // 电机状态回调
    void motorStateCallback(const customized_msgs::msg::FeedBack::SharedPtr msg)
    {
        // Handle motor state message
        motor_state_ready_flag = true;
        motor_state_msg = *msg;
    }
    // 地面高度回调 数据来源是小飞机的里程计
    // void groundHeightCallback(const customized_msgs::msg::GroundDetection::SharedPtr msg)
    // {
    //     if (msg->variance > 0.1)
    //     {
    //         RCLCPP_INFO(this->get_logger(), "height variance is too high: %f, skip this message", msg->variance);
    //         return;
    //     }
    //     ground_height_init_flag = true;
    //     ground_height_msg = *msg;
    // }

    // void swimming_torque_update(double elapse_time)
    // {
    //     double high_torque_ratio = 0.1;
    //     double loop_time = 1.0;

    //     double rest_time = std::fmod(elapse_time, loop_time);
    //     if (rest_time < high_torque_ratio * loop_time)
    //     {
    //         swimming_torque = SWIM_TORQUE;
    //     }
    //     else
    //     {
    //         swimming_torque = REF_TORQUE;
    //     }
    // }
    // 发布四号电机的力矩
    void publish_motor_cmd(customized_msgs::msg::MultiCanMotorCmd motor_cmd)
    {
        motor_cmd.multi_can_motor_cmd[1].ref_torque = motor_cmd.multi_can_motor_cmd[1].ref_torque * reserve_flag;
        winch_ctrl_pub->publish(motor_cmd);
    }
    // 电机角度速度置零
    void setZeroCallback(const std_msgs::msg::Empty::SharedPtr msg)
    {
        target_angle = 0;
        motor_cmd.multi_can_motor_cmd[0].target_angle = 0;
        motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
        RCLCPP_WARN(this->get_logger(), "set_zero_cb!");
    }
    // 电机角度设置回调
    void setAngleCallback(const customized_msgs::msg::ArgSetAngle::SharedPtr msg)
    {
        if (set_angle_msg.angle == 0)
        {
            set_angle_msg = *msg;
            return;
        }
        else
        {
            // If new angle is greater than old angle
            if (msg->angle - set_angle_msg.angle > 0.0)
            {
                motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
            }
            else
            {
                motor_cmd.multi_can_motor_cmd[1].ref_torque = -BACK_TORQUE;
            }
        }

        // Convert angle to radians
        set_angle_msg = *msg;
        motor_cmd.multi_can_motor_cmd[0].target_angle = set_angle_msg.angle * M_PI / 180.0;
        motor_cmd.multi_can_motor_cmd[0].target_velocity = set_angle_msg.speed * M_PI / 180.0;
        target_angle = set_angle_msg.angle * M_PI / 180.0;

        publish_motor_cmd(motor_cmd);
    }
    // 电机长度设置回调
    void setLengthCallback(const customized_msgs::msg::ArgSetLength::SharedPtr msg)
    {
        if(msg->abs_ctrl == 0)
        {
            return;
        }
        // Length to meter
        set_length_msg = *msg;
        last_receive_relative_set_length_time = this->now();

        if (set_length_msg.abs_ctrl == 1)
        {
            target_abs_angle = meter_to_angle(set_length_msg.length);

            back_start_angle = target_angle;
            back_start_length = angle_to_meter(target_abs_angle);

            RCLCPP_INFO(this->get_logger(), "back_start_angle = %f", back_start_angle);
            RCLCPP_INFO(this->get_logger(), "back_start_length = %f", back_start_length);

            change_control_state("ABS_LENGTH");
        }
        else
        {
            // RCLCPP_INFO(this->get_logger(), "relative length control: %f", set_length_msg.length);

            // target_angle = angle_plus_meter_to_angle(target_angle, set_length_msg.length);

            // change_control_state("RELATIVE_LENGTH");
        }
    }
    // 小飞机里程计回调
    void droneOdomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
    {
        // Handle odometry message
        drone_odom = *msg;
    }

    void virtualRcCallback(const std_msgs::msg::Float32::SharedPtr msg)
    {
        if (!real_rc_override_flag)
        {
            rc_in = msg->data;
            if (fabs(rc_in) > 0.01)
                change_control_state("RC");
        }
    }

    void rcInCallback(const mavros_msgs::msg::RCIn::SharedPtr msg)
    {
        rc_msg = *msg;
        real_rc_override_flag = false;

        rclcpp::Time now = this->now();
        double interval = (now - last_rc_time).seconds();
        // 检查超时
        if (rc_interval == 0.0)
        {
            RCLCPP_INFO(this->get_logger(), "rc_in_cb init");
            last_rc_time = now;
            rc_interval = 0.01;
            return;
        }

        if (interval > 0.2)
        {
            RCLCPP_ERROR(this->get_logger(), "rc_in_cb time interval too large");
            rc_lost_flag = true;
            last_rc_time = now;
            return;
        }

        rc_interval = (interval > 0.15) ? 0.1 : interval;
        last_rc_time = now;

        int channel_8 = rc_msg.channels[8];

        if (std::abs(channel_8 - 1500) < DEAD_ZONE)
        {
            init_middle_flag = true;
            rc_in = 0;
        }
        else if (!init_middle_flag)
        {
            RCLCPP_WARN(this->get_logger(), "You should place your channel 9 at middle first!");
        }
        else
        {
            rc_in = (channel_8 > 1500) ? (channel_8 - 1500 - DEAD_ZONE) / (500.0 - DEAD_ZONE)
                                       : (channel_8 - 1500 + DEAD_ZONE) / (500.0 - DEAD_ZONE);
            real_rc_override_flag = true;
            rc_in = std::clamp(rc_in, -1.0, 1.0);
            if (rc_in != 0)
                change_control_state("RC");
        }

        if (init_middle_flag)
        {
            if (rc_msg.channels[5] < 1000)
            {
                std_msgs::msg::Empty empty_msg;
                set_zero_pub->publish(empty_msg);
                RCLCPP_INFO(this->get_logger(), "publish set_zero msg");
            }

            // if (rc_msg.channels[10] > 1500 && control_state != 2)
            // {
            //     rappel_near_ground();
            // }
            else if (rc_msg.channels[13] > 1500 && control_state != 1)
            {
                change_control_state("ABS_LENGTH");
                RCLCPP_INFO(this->get_logger(), "go zero mode");
                velocity = 0;
                target_abs_angle = 0;
                back_start_angle = target_angle;
                back_start_length = angle_to_meter(target_abs_angle);

                RCLCPP_INFO(this->get_logger(), "go zero start_angle = %f", back_start_angle);
                RCLCPP_INFO(this->get_logger(), "go zero start_length = %f", back_start_length);
            }
            else if (rc_msg.channels[12] > 1500 && control_state != 5)
            {
                change_control_state("TOUCH_GROUND");
            }
        }
    }
    bool check_double_equal(double a,double b)
    {
        const double precision = 0.00001;
        if(fabs(a-b) < precision)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    // 小飞机状态回调 检查是否看见目标
    void droneStateCallback(const customized_msgs::msg::DroneWholeState::SharedPtr msg)
    {

        last_droneState_time_ = rclcpp::Clock().now();
        drone_state_msg = *msg;

        if (!check_double_equal(msg->odom_x, uninitialized_double) || !check_double_equal(msg->odom_y, uninitialized_double) || !check_double_equal(msg->odom_z, uninitialized_double))
        // if (odom_x != uninitialized_double || odom_y != uninitialized_double || odom_z != uninitialized_double)
        {
            grabber_init_flag = true;
        }
    }
    // 自动放绳
    void rappel_near_ground()
    {

       
        RCLCPP_INFO(this->get_logger(), "auto rappel near ground");
        RCLCPP_INFO(this->get_logger(), "into rappel_near_ground  now length= %f", angle_to_meter(target_angle));
        RCLCPP_INFO(this->get_logger(), "target %f", fly_height - reservedHeight - 0.05);
        change_control_state("RAPPEL");
    }

    void controlLoopCallback()
    {
        current_length = angle_to_meter(target_angle);

        customized_msgs::msg::WinchState winch_state_msg;
        winch_state_msg.stamp = this->now();
        winch_state_msg.target_angle = target_angle;
        winch_state_msg.target_length = current_length;
        winch_state_msg.control_state = control_state;
        winch_state_msg.target_velocity = motor_cmd.multi_can_motor_cmd[0].target_velocity;
        winch_state_pub->publish(winch_state_msg);
        // rclcpp::Time ground_height_time(ground_height_msg.stamp);

        static rclcpp::Time last_time = this->now();
        rclcpp::Time current_time = this->now();
        double delta_t = (current_time - last_time).seconds();
        last_time = current_time;
        if (delta_t == 0)
            return;
        // 高度信息是否超时
        // if (ground_height_init_flag && (current_time - ground_height_time).seconds() > 1.0)
        // {
        //     ground_height_init_flag = false;
        //     RCLCPP_INFO(this->get_logger(), "ground height timeout!!");
        // }

        // swimming_torque_update((current_time - init_time).seconds());

        if (!set_length_msg.abs_ctrl && (current_time - last_receive_relative_set_length_time).seconds() < set_length_msg.length / set_length_speed)
        {
            change_control_state("RELATIVE_LENGTH");
        }
        // if (autoRopeRelease == true)
        // {
        //     rappel_near_ground();
        // }
        switch (control_state)
        {
        case 7:
        {
            motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
            RCLCPP_WARN(this->get_logger(), "EMERGENCY STOP!!!!!");
            break;
        }
        case 0: // RC mode
        {
            motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
            // 触发限位 不允许回收
            if (go_up_lock == true && rc_in < 0)
            {
                rc_in = 0;
                RCLCPP_WARN(this->get_logger(), "go up lock enabled! rc_in forced set to 0");
            }

            if (rc_in != 0)
            {
                // 快收到底，限制速度
                double limited_MANUAL_SPEED = MANUAL_SPEED;
                // if (current_length <  0.5 && rc_in < 0)
                //  {
                //      limited_MANUAL_SPEED = 0.1/radius;
                //  }
                //  else if (current_length < 1.0 && rc_in < 0)
                //  {
                //      limited_MANUAL_SPEED = 0.2/radius;
                //  }

                motor_cmd.multi_can_motor_cmd[0].target_velocity = std::abs(rc_in) * limited_MANUAL_SPEED;
                target_angle += rc_in * limited_MANUAL_SPEED * delta_t;
                idle_speed = std::abs(rc_in) * limited_MANUAL_SPEED;
                if (idle_speed < 5)
                {
                    idle_speed = 5;
                }
                motor_cmd.multi_can_motor_cmd[1].ref_torque = (rc_in > 0) ? swimming_torque : -BACK_TORQUE;
            }
            else
            {
                // 保持上一秒状态，但是也会导致初始化的时候给了一个力矩
                // 每次保持上一次状态一次 下次就置零
                if (swimming_torque_state == false)
                {
                    motor_cmd.multi_can_motor_cmd[0].target_velocity = idle_speed;
                    motor_cmd.multi_can_motor_cmd[1].ref_torque = 0;
                }
                else
                {
                    motor_cmd.multi_can_motor_cmd[0].target_velocity = idle_speed;
                    motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                    swimming_torque_state = false;
                }
            }
            break;
        }

        case 1: // ABS length mode
        {
            if (!abs_length_flag)
            {
                abs_length_flag = true;
                RCLCPP_INFO(this->get_logger(), "ABS length mode");
            }
            else
            {
                double remain_direction = target_angle - target_abs_angle;
                double remain_angle = std::abs(target_angle - target_abs_angle);
                double total_angle = std::abs(back_start_angle - target_abs_angle);
                double acc_angle = max_velocity * max_velocity / (2.0 * accleration);
                // v**2=u**2+2as v是最终速度 u是初速度(假设为0) a是加速度 s是距离 移相得到s 这里电机做圆周运动 s是角度
                double plus_flag = (target_angle > target_abs_angle) ? -1 : 1;

                if (go_up_lock == true && remain_direction > 0) // remainn_direction > 0 说明回收
                {
                    remain_angle = 0;
                    RCLCPP_WARN(this->get_logger(), "go up lock enabled! remain_angle forced set to 0");
                }

                // 角度误差在0.05度以内，则认为已经到位
                if (remain_angle < 0.05)
                {
                    abs_length_flag = false;
                    change_control_state("RC");
                    velocity = min_velocity;
                    RCLCPP_INFO(this->get_logger(), "ABS length mode end");
                    break;
                }
                // 根据绳长修改速度
                // else if (总角度大于两倍的加速角度
                //   if（剩余角度>总角度-加速角度）
                //       {增加速度}
                //    else{减少速度}

                // else如果总角度不大于两倍的加速角度，
                // 那么 if (剩余角度 > 总角度的一半)
                //  {
                //      加速
                //  }
                //  else
                //  {
                //      减速
                //  }
                else if (total_angle > 2 * acc_angle)
                {
                    if (remain_angle > total_angle - acc_angle)
                    {
                        velocity += accleration * delta_t;
                    }
                    else if (remain_angle < acc_angle)
                    {
                        velocity -= accleration * delta_t;
                    }
                }
                else
                {
                    if (remain_angle > total_angle / 2)
                    {
                        velocity += accleration * delta_t;
                    }
                    else
                    {
                        velocity -= accleration * delta_t;
                    }
                }
                // 控制速度范围
                velocity = std::clamp(velocity, min_velocity, max_velocity);

                target_angle += velocity * delta_t * plus_flag; // plus_flag速度方向

                motor_cmd.multi_can_motor_cmd[0].target_velocity = velocity;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_abs_angle;
                motor_cmd.multi_can_motor_cmd[1].ref_torque = (plus_flag == -1) ? -BACK_TORQUE : swimming_torque;
                idle_speed = velocity; // 这个是干什么的？？？
            }
            break;
        }
        case 2: // Rappel mode
        {
            motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
            bool target_flag = false;
            auto now = rclcpp::Clock().now();
            auto duration = now - last_droneState_time_;
            if (duration.seconds() > 1.0 ||  !grabber_init_flag )
            { // 通信超时 切换到rc模式并且停止卷扬机
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                grabber_init_flag = false;
                change_control_state("RC");
                RCLCPP_INFO(this->get_logger(), " Communication timeout!! switch control mode from Rapple to rc ");
            }
            else
            {
                if (drone_state_msg.tag_seen_flag == 1)
                {
                    // target_flag=true;
                    // 未考虑地平线以下的情况
                    // fly_height - angle_to_meter(target_angle)
                    if (grabber_init_flag)
                    {

                        double fly_tag_distance = drone_state_msg.odom_z - drone_state_msg.tag_z ;
                        RCLCPP_INFO(this->get_logger(), "distance %f", fly_tag_distance);
                        if ((tag_optimal_distance - 0.05 <= fly_tag_distance) && (fly_tag_distance <= tag_optimal_distance + 0.05))
                        {
                            // if (tag_optimal_distance-0.1<= fly_height-angle_to_meter(target_angle)) && (fly_height-angle_to_meter(target_angle)<=tag_optimal_distance+0.1){
                            // 目标的 正负0.05
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                            change_control_state("RC");
                            RCLCPP_INFO(this->get_logger(), " Target found!!! control mode from Rapple to rc ");
                        }
                        else if (fly_tag_distance > tag_optimal_distance + 0.1)
                        { // 距离远 放绳
                            int speed_mapping = static_cast<int>(round(fly_tag_distance / 2));
                            speed_mapping = std::max(4, std::min(speed_mapping, 5)); // 四舍五入到【1，4】
                            double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                            target_angle += limited_rappel_SPEED * delta_t;
                            motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                        }
                        else if (fly_tag_distance < tag_optimal_distance - 0.1) // 太近了 收绳
                        {

                            int speed_mapping = static_cast<int>(round(fly_tag_distance / 2));
                            speed_mapping = -std::max(4, std::min(speed_mapping, 5)); // 四舍五入到-1 -4
                            double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                            target_angle += limited_rappel_SPEED * delta_t;
                            motor_cmd.multi_can_motor_cmd[1].ref_torque = -BACK_TORQUE;
                        }
                    }
                    else
                    {
                        motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                        grabber_init_flag = false;
                        change_control_state("RC");
                        RCLCPP_INFO(this->get_logger(), " Uninitialized!!! switch control mode from Rapple to rc ");
                    }
                }
                else
                { // 开环控制 （其实飞机高度和绳长都不准的）
                    if (angle_to_meter(target_angle) <= fly_height - reservedHeight - 0.05)
                    { // 未到限制高度
                        int speed_mapping = static_cast<int>(round((fly_height - angle_to_meter(target_angle)) / 3));
                        speed_mapping = std::max(4, std::min(speed_mapping, 5));
                        double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                        motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                        target_angle += limited_rappel_SPEED * delta_t;
                        motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                    }
                    else
                    { // 下放到限制高度还没看见目标
                        motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                        change_control_state("RC");
                        RCLCPP_INFO(this->get_logger(), " Target not found!!! control mode from Rapple to rc ");
                    }
                }
            }
            break;
        }
            /*case 2: // Rappel mode
            {
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;

                bool target_flag = false;
                auto now = rclcpp::Clock().now();
                auto duration = now - last_droneState_time_;
                if (duration.seconds() > 1.0)
                { // 通信超时 切换到rc模式并且停止卷扬机
                    motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                    change_control_state("RC");
                    RCLCPP_INFO(this->get_logger(), " Communication timeout!! switch control mode from Rapple  to rc ");
                    break;
                }
                else
                {
                    if (drone_state_msg.tag_seen_flag == 1)
                    {
                        // target_flag=true;
                        // 未考虑地平线以下的情况
                        if ((tag_optimal_distance - 0.1 <= fly_height - angle_to_meter(target_angle)) && (fly_height - angle_to_meter(target_angle) <= tag_optimal_distance + 0.1))
                        {
                            // if (tag_optimal_distance-0.1<= fly_height-angle_to_meter(target_angle)) && (fly_height-angle_to_meter(target_angle)<=tag_optimal_distance+0.1){
                            // 目标的 正负0.1
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                            change_control_state("RC");
                            RCLCPP_INFO(this->get_logger(), " Target  found!!! control mode from Rapple  to rc ");
                        }
                        else if (fly_height - angle_to_meter(target_angle) > tag_optimal_distance + 0.1)
                        { // 距离远 放绳
                            int speed_mapping = static_cast<int>(round((drone_state_msg.tag_z - tag_optimal_distance) / 3));
                            speed_mapping = std::max(1, std::min(speed_mapping, 4)); // 四舍五入到【1，4】
                            double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                            target_angle += limited_rappel_SPEED * delta_t;
                            motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                        }
                        else if (fly_height - angle_to_meter(target_angle) < tag_optimal_distance - 0.1)
                        { // 太近了 收绳
                            int speed_mapping = static_cast<int>(round((tag_optimal_distance - drone_state_msg.tag_z) / 3));
                            speed_mapping = -std::max(1, std::min(speed_mapping, 4)); // 四舍五入到-1 -4
                            double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                            target_angle += limited_rappel_SPEED * delta_t;
                            motor_cmd.multi_can_motor_cmd[1].ref_torque = -BACK_TORQUE;
                        }
                    }
                    else
                    {
                        if (angle_to_meter(target_angle) <= fly_height - reservedHeight - 0.05)
                        { // 未到限制高度
                            int speed_mapping = static_cast<int>(round((fly_height - reservedHeight) / 5));
                            speed_mapping = std::max(1, std::min(speed_mapping, 4));
                            double limited_rappel_SPEED = 0.2 / radius * speed_mapping;
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = limited_rappel_SPEED;
                            target_angle += limited_rappel_SPEED * delta_t;
                            motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                        }
                        else
                        { // 下放到限制高度还没看见目标
                            motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                            change_control_state("RC");
                            RCLCPP_INFO(this->get_logger(), " Target not found!!! control mode from Rapple  to rc ");
                        }
                    }
                }
                break;
            }*/

        // case 3: // Hold mode
        // {
        //     double error_height = drone_odom.pose.pose.position.z - ground_height_msg.filtered_height - ground_away_distance;
        //     if (error_height < -0.1)
        //     {
        //         target_angle -= hold_speed * delta_t;
        //         motor_cmd.multi_can_motor_cmd[1].ref_torque = -BACK_TORQUE;
        //     }
        //     else if (error_height > 0.1)
        //     {
        //         target_angle += hold_speed * delta_t;
        //         motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
        //     }
        //     motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
        //     motor_cmd.multi_can_motor_cmd[0].target_velocity = hold_speed;
        //     idle_speed = hold_speed;
        //     break;
        // }

        case 4: // Relative length mode
        {
            motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
            motor_cmd.multi_can_motor_cmd[0].target_velocity = set_length_speed;
            motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
            break;
        }

        case 5: // Rappel touch ground mode
        {
            double critical_torque = -1.3;
            if (!motor_state_ready_flag)
            {
                RCLCPP_WARN(this->get_logger(), "Waiting for motor ready");
                change_control_state("RC");
            }
            else if (motor_state_msg.feedback_torque < critical_torque)
            {
                last_feedback_torque_touch_ground = motor_state_msg.feedback_torque;
                target_angle += hold_speed * delta_t;
                motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = hold_speed;
                idle_speed = hold_speed;
            }
            else
            {
                if (last_feedback_torque_touch_ground < critical_torque)
                {
                    target_angle += 0.3; // extra length
                    motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                    motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                    motor_cmd.multi_can_motor_cmd[0].target_velocity = hold_speed;
                    idle_speed = hold_speed;
                    last_feedback_torque_touch_ground = 0;
                    RCLCPP_WARN(this->get_logger(), "Touch ground and release extra length of 0.3 rad");
                }
                RCLCPP_INFO(this->get_logger(), "Touch the ground, stop");
                change_control_state("RC");
            }
            break;
        }

        case 6:
        {
            if (PRECISE_ZERO_MODE == 0)
                precise_zero_process_by_torque();
            else if (PRECISE_ZERO_MODE == 1)
                precise_zero_process_by_tof();
            break;
        }

        case 8: // catch up
        {
            double critical_torque = -5.0;
            if (!motor_state_ready_flag)
            {
                RCLCPP_WARN(this->get_logger(), "waiting for motor ready");

                // control_state = 0;
                change_control_state("RC");
            }
            else if (motor_state_msg.feedback_torque > critical_torque)
            {
                target_angle -= 0.2 * hold_speed * delta_t;
                motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 0.2 * hold_speed;
                idle_speed = 0.2 * hold_speed;
            }
            else
            {
                RCLCPP_WARN(this->get_logger(), "catch up successfully, stop  target_angle set from %f to %f", target_angle, motor_state_msg.feedback_angle);
                target_angle = motor_state_msg.feedback_angle;
                motor_cmd.multi_can_motor_cmd[1].ref_torque = swimming_torque;
                motor_cmd.multi_can_motor_cmd[0].target_angle = target_angle;
                motor_cmd.multi_can_motor_cmd[0].target_velocity = 0;
                // control_state = 0;
                idle_speed = 0;
                change_control_state("RC");
            }
            break;
        }
        default:
        {
            RCLCPP_WARN(this->get_logger(),"unknown case ");
            break;
        }
        }

        publish_motor_cmd(motor_cmd);
    }

    // void updateVelCallback()
    // {
    //     // Update velocity timer callback function
    // }
private:
    // Member variables
    // void precise_zero_process_by_tof();
    // void precise_zero_process_by_torque();
    // double angle_to_meter(double angle);
    // double meter_to_angle(double meter);
    // void change_control_state(std::string state_str, bool override_flag = false) void change_control_state(int target_state);
    // double angle_plus_meter_to_angle(double angle, double plus_meter);
    // void swimming_torque_update(double elapse_time);
    // void rappel_near_ground();
    // void publish_motor_cmd(customized_msgs::msg::MultiCanMotorCmd motor_cmd);
    // void rappel_near_ground();

    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr drone_odom_sub;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr set_zero_sub;
    rclcpp::Subscription<customized_msgs::msg::FeedBack>::SharedPtr motor_state_sub;
    rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr virtual_rc_sub;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr precise_zero_sub;
    rclcpp::Subscription<customized_msgs::msg::ArgSetAngle>::SharedPtr set_angle_sub;
    rclcpp::Subscription<mavros_msgs::msg::RCIn>::SharedPtr rc_in_sub;
    rclcpp::Subscription<customized_msgs::msg::ArgSetLength>::SharedPtr set_length_sub;
    // rclcpp::Subscription<customized_msgs::msg::GroundDetection>::SharedPtr ground_height_sub;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr rappel_near_ground_sub, rappel_touch_ground_sub, catch_up_sub;
    rclcpp::Subscription<sensor_msgs::msg::Range>::SharedPtr tof_dis_sub;
    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr motor_reset_sub, emergency_stop_sub;
    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr limit_switch_sub;
    rclcpp::Subscription<customized_msgs::msg::DroneWholeState>::SharedPtr drone_state_sub;

    rclcpp::Publisher<customized_msgs::msg::MultiCanMotorCmd>::SharedPtr winch_ctrl_pub;
    rclcpp::Publisher<customized_msgs::msg::WinchState>::SharedPtr winch_state_pub;
    rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr set_zero_pub;

    rclcpp::TimerBase::SharedPtr timer1, update_vel_timer;

    nav_msgs::msg::Odometry drone_odom;
    customized_msgs::msg::MultiCanMotorCmd motor_cmd;
    mavros_msgs::msg::RCIn rc_msg;
    rclcpp::Time last_rc_time, init_time, last_update_pos_time, last_receive_relative_set_length_time = this->now();
    customized_msgs::msg::ArgSetAngle set_angle_msg;
    customized_msgs::msg::ArgSetLength set_length_msg;
    sensor_msgs::msg::Range tof_dis_msg;

    bool real_rc_override_flag = false;
    double rc_interval = 0.0, swimming_torque = 0.9, BACK_TORQUE = -0.3, diff_vel = 0.0, target_abs_angle = 0.0;
    double last_pos = 0.0, controller_freq = 0.0, delta_time = 0.0, MANUAL_SPEED = 0.0, accleration = 0.0, velocity = 0.0, max_velocity = 0.0, min_velocity = 0.0, hold_speed = 0.0, set_length_speed = 0.0;
    double fly_height = 20, reservedHeight = 4, tag_optimal_distance = 3; //
    double idle_speed = 10.0, radius = 0.05, target_angle = 0.0, current_length = 0.0, DEAD_ZONE = 200, rc_in = 0.0;
    double back_start_angle = 0.0, back_start_length = 0.0, rappel_start_angle = 0.0, rappel_target_angle = 0.0, rappel_start_drone_height = 0.0;
    bool init_middle_flag = false, abs_length_flag = false, go_rappel_flag = false;
    int control_state = 0, reserve_flag = -1;
    std::vector<std::string> control_state_str = {"RC", "ABS_LENGTH", "RAPPEL", "HOLD", "RELATIVE_LENGTH", "TOUCH_GROUND", "PRECISE_ZERO", "EMERGENCY_STOP", "CATCH_UP"};
    bool rc_lost_flag = false, motor_state_ready_flag = false;
    customized_msgs::msg::FeedBack motor_state_msg;
    // customized_msgs::msg::GroundDetection ground_height_msg;
    customized_msgs::msg::DroneWholeState drone_state_msg;

    double ground_away_distance = 0.5;

    double last_feedback_torque_touch_ground = 0.0;

    double critical_d_torque = -1.0;
    double precise_zero_step_min = 0.002;
    double precise_zero_step_max = 0.01;

    bool tof_ready_flag = false;

    bool last_precise_zero_flag = false, precise_zero_finished_flag = false;
    double precise_zero_init_torque = 0.0;

    bool go_up_lock = false;
    bool swimming_torque_state = false;
    bool autoRopeRelease = false;
    bool grabber_init_flag=false;
    const double uninitialized_double = 0.12;
    rclcpp::Time last_droneState_time_ = rclcpp::Clock().now();
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<WinchController>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
